/* helloworld.c for TOPPERS/ATK(OSEK) */
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void)
{ /* do nothing */
}

// Gebaut bis zur 17 :)

// Port 1: Unused
// Port 2: Unused
// Port 3: Color sensor
// Port 4: Unused

// Port A: Steering motor
// Port B: Driving motor back left
// Port C: Driving motor back right

TASK(OSEK_Task_Background)
{
   while (1)
   {
      if (ecrobot_is_ENTER_button_pressed())
      {
         ecrobot_status_monitor("Driving...");
         ecrobot_set_motor_speed(NXT_PORT_B, -100);
         ecrobot_set_motor_speed(NXT_PORT_C, -100);
         systick_wait_ms(10000);
      }
      else
      {
         ecrobot_set_motor_speed(NXT_PORT_B, 0);
         ecrobot_set_motor_speed(NXT_PORT_C, 0);
         ecrobot_status_monitor("STOP");
      }
      systick_wait_ms(10);
   }
}
